#ifndef _AMERICO_MATRIX33_H_
#define _AMERICO_MATRIX33_H_

#include <cmath>
#include "staticarray.h"
#include "Vector3.h"

// Matrix33 Declaration
template <typename TYPE>
class Matrix33
{
public:
	Matrix33();
	Matrix33(const Matrix33<TYPE> &aMatrix);
	Matrix33(const Vector3<TYPE> &aLeftVector, const Vector3<TYPE> &anUpVector, const Vector3<TYPE> &aForwardVector);
	~Matrix33();

	Matrix33 &operator=(const Matrix33 &aMatrix);

	bool operator==(const Matrix33 &aMatrix);
	bool operator!=(const Matrix33 &aMatrix);

	const TYPE &operator[](const unsigned int &anIndex) const;
	TYPE &operator[](const unsigned int &anIndex);

	Matrix33 operator-() const;

	Matrix33 operator+(const Matrix33 &aMatrix) const;
	Matrix33 operator-(const Matrix33 &aMatrix) const;
	Matrix33 operator*(const Matrix33 &aMatrix) const;
	Matrix33 operator*(const TYPE &aScalar) const;

	Vector3<TYPE> operator*(const Vector3<TYPE> &aVector) const;

	Matrix33 &operator+=(const Matrix33 &aMatrix);
	Matrix33 &operator-=(const Matrix33 &aMatrix);
	Matrix33 &operator*=(const Matrix33 &aMatrix);
	Matrix33 &operator*=(const TYPE &aScalar);

	Matrix33 operator~() const;

	void Init(const TYPE someData[9]);
	void Init(const TYPE &a11, const TYPE &a12, const TYPE &a13,
		const TYPE &a21, const TYPE &a22, const TYPE &a23,
		const TYPE &a31, const TYPE &a32, const TYPE &a33);

	static Matrix33 CreateRotateAroundX(const TYPE &anAngleInRadians);
	static Matrix33 CreateRotateAroundY(const TYPE &anAngleInRadians);
	static Matrix33 CreateRotateAroundZ(const TYPE &anAngleInRadians);
	static Matrix33 CreateRotate(const Vector3<TYPE> &aVector);
	static Matrix33 RotationAroundVector(const Vector3<TYPE> &aVector, const TYPE &anAngleInRadians);

	static Matrix33 Identity();

private:
	Utils::StaticArray<TYPE, 3*3> myMatrix;
};

// Matrix33 Definition
template <typename TYPE>
Matrix33<TYPE>::Matrix33()
{
	myMatrix[0] = myMatrix[4] = myMatrix[8] = 1;
	myMatrix[1] = myMatrix[2] = myMatrix[3] = 0;
	myMatrix[5] = myMatrix[6] = myMatrix[7] = 0;
}

template <typename TYPE>
Matrix33<TYPE>::Matrix33(const Matrix33<TYPE> &aMatrix)
{
	myMatrix = aMatrix.myMatrix;
}

template <typename TYPE>
Matrix33<TYPE>::Matrix33(const Vector3<TYPE> &aLeftVector, const Vector3<TYPE> &anUpVector, const Vector3<TYPE> &aForwardVector)
{
	myMatrix[0] = aLeftVector.x;
	myMatrix[1] = aLeftVector.y;
	myMatrix[2] = aLeftVector.z;
	myMatrix[3] = anUpVector.x;
	myMatrix[4] = anUpVector.y;
	myMatrix[5] = anUpVector.z;
	myMatrix[6] = aForwardVector.x;
	myMatrix[7] = aForwardVector.y;
	myMatrix[8] = aForwardVector.z;
}

template <typename TYPE>
Matrix33<TYPE>::~Matrix33()
{
}

template <typename TYPE>
Matrix33<TYPE> &Matrix33<TYPE>::operator =(const Matrix33<TYPE> &aMatrix)
{
	for(unsigned int index=0; index<9; ++index)
	{
		myMatrix[index] = aMatrix.myMatrix[index];
	}
	return *this;
}

template <typename TYPE>
bool Matrix33<TYPE>::operator ==(const Matrix33<TYPE> &aMatrix)
{
	for(unsigned int index=0; index<9; ++index)
	{
		if(myMatrix[index] != aMatrix.myMatrix[index])
		{
			return false;
		}
	}
	return true;
}

template <typename TYPE>
bool Matrix33<TYPE>::operator !=(const Matrix33<TYPE> &aMatrix)
{
	return !(operator ==(aMatrix));
}

template <typename TYPE>
const TYPE &Matrix33<TYPE>::operator [](const unsigned int &anIndex) const
{
	return myMatrix[anIndex];
}

template <typename TYPE>
TYPE &Matrix33<TYPE>::operator [](const unsigned int &anIndex)
{
	return myMatrix[anIndex];
}

template <typename TYPE>
Matrix33<TYPE> Matrix33<TYPE>::operator -() const
{
	Matrix33<TYPE> result(*this);
	for (unsigned int index=0; index<myMatrix.size(); ++index)
	{
		result[index] = -myMatrix[index];
	}
	return result;
}

template <typename TYPE>
Matrix33<TYPE> Matrix33<TYPE>::operator +(const Matrix33<TYPE> &aMatrix) const
{
	Matrix33<TYPE> result(*this);
	result += aMatrix;
	return result;
}

template <typename TYPE>
Matrix33<TYPE> Matrix33<TYPE>::operator -(const Matrix33<TYPE> &aMatrix) const
{
	Matrix33<TYPE> result(*this);
	result -= aMatrix;
	return result;
}

template <typename TYPE>
Matrix33<TYPE> Matrix33<TYPE>::operator *(const Matrix33<TYPE> &aMatrix) const
{
	Matrix33<TYPE> result(*this);
	result *= aMatrix;
	return result;
}

template <typename TYPE>
Matrix33<TYPE> Matrix33<TYPE>::operator *(const TYPE &aScalar) const
{
	Matrix33<TYPE> result(*this);
	result *= aScalar;
	return result;
}

template <typename TYPE>
Vector3<TYPE> Matrix33<TYPE>::operator *(const Vector3<TYPE> &aVector) const
{
	Vector3<TYPE> result(0,0,0);
	result.x = aVector.myX*myMatrix[0] + aVector.myY*myMatrix[3] + aVector.myZ*myMatrix[6];
	result.y = aVector.myX*myMatrix[1] + aVector.myY*myMatrix[4] + aVector.myZ*myMatrix[7];
	result.z = aVector.myX*myMatrix[2] + aVector.myY*myMatrix[5] + aVector.myZ*myMatrix[8];
	return result;
}

template <typename TYPE>
Matrix33<TYPE> &Matrix33<TYPE>::operator +=(const Matrix33<TYPE> &aMatrix)
{
	for(unsigned int index=0; index<9; ++index)
	{
		myMatrix[index] += aMatrix.myMatrix[index];
	}
	return *this;
}

template <typename TYPE>
Matrix33<TYPE> &Matrix33<TYPE>::operator -=(const Matrix33<TYPE> &aMatrix)
{
	for(unsigned int index=0; index<9; ++index)
	{
		myMatrix[index] -= aMatrix.myMatrix[index];
	}
	return *this;
}

template <typename TYPE>
Matrix33<TYPE> Matrix33<TYPE>::operator~() const
{
	Matrix33<TYPE> result;
	for(unsigned int i=0; i<3; ++i)
	{
		for(unsigned int j=0; j<3; ++j)
		{
			result.myMatrix[i + j*3] = myMatrix[j + i*3];
		}
	}
	return result;
}

template <typename TYPE>
Matrix33<TYPE> &Matrix33<TYPE>::operator *=(const Matrix33<TYPE> &aMatrix)
{
	Matrix33<TYPE> matrix(*this);
	for(unsigned int i=0; i<3; ++i)
	{
		for(unsigned int j=0; j<3; ++j)
		{
			myMatrix[i + 3*j] = 0;
			for(unsigned int r=0; r<3; ++r)
			{
#ifdef MATRIX_COLUMN
				myMatrix[i + 3*j] += matrix.myMatrix[r + 3*i] * aMatrix.myMatrix[j + 3*r];
#else
				myMatrix[i + 3*j] += matrix.myMatrix[i + 3*r] * aMatrix.myMatrix[r + 3*j];
#endif
			}
		}
	}
	return *this;
}

template <typename TYPE>
Matrix33<TYPE> &Matrix33<TYPE>::operator *=(const TYPE &aScalar)
{
	Matrix33<TYPE> matrix(*this);
	for(unsigned int i=0; i<3*3; ++i)
	{
		myMatrix[i] *= aScalar;
	}
	return *this;
}

template <typename TYPE>
void Matrix33<TYPE>::Init(const TYPE someData[9])
{
	for(unsigned int index=0; index<9; ++index)
	{
		myMatrix[index] = someData[index];
	}
}

template <typename TYPE>
void Matrix33<TYPE>::Init(const TYPE &a11, const TYPE &a12, const TYPE &a13,
						  const TYPE &a21, const TYPE &a22, const TYPE &a23,
						  const TYPE &a31, const TYPE &a32, const TYPE &a33)
{
	myMatrix[0] = a11;
	myMatrix[1] = a12;
	myMatrix[2] = a13;

	myMatrix[3] = a21;
	myMatrix[4] = a22;
	myMatrix[5] = a23;

	myMatrix[6] = a31;
	myMatrix[7] = a32;
	myMatrix[8] = a33;
}

template <typename TYPE>
Matrix33<TYPE> Matrix33<TYPE>::CreateRotateAroundX(const TYPE &anAngleInRadians)
{
	Matrix33<TYPE> result;
	result.myMatrix[4] = result.myMatrix[8] = cos(anAngleInRadians);
	result.myMatrix[7] = sin(anAngleInRadians);
	result.myMatrix[5] = -result.myMatrix[7];
	return result;
}

template <typename TYPE>
Matrix33<TYPE> Matrix33<TYPE>::CreateRotateAroundY(const TYPE &anAngleInRadians)
{
	Matrix33<TYPE> result;
	result.myMatrix[0] = result.myMatrix[8] = cos(anAngleInRadians);
	result.myMatrix[2] = sin(anAngleInRadians);
	result.myMatrix[6] = -result.myMatrix[2];
	return result;
}

template <typename TYPE>
Matrix33<TYPE> Matrix33<TYPE>::CreateRotateAroundZ(const TYPE &anAngleInRadians)
{
	Matrix33<TYPE> result;
	result.myMatrix[0] = result.myMatrix[4] = cos(anAngleInRadians);
	result.myMatrix[3] = sin(anAngleInRadians);
	result.myMatrix[1] = -result.myMatrix[3];
	return result;
}

template <typename TYPE>
Matrix33<TYPE> Matrix33<TYPE>::CreateRotate(const Vector3<TYPE> &aVector)
{
	return CreateRotateAroundY(aVector.y) * CreateRotateAroundX(aVector.x) * CreateRotateAroundZ(aVector.z);
}

template <typename TYPE>
Matrix33<TYPE> Matrix33<TYPE>::RotationAroundVector( const Vector3<TYPE> &aVector, const TYPE &anAngleInRadians )
{
	Matrix33<TYPE> result;
	float cosAngle = cos(anAngleInRadians);
	float sinAngle = sin(anAngleInRadians);

	result.myMatrix[0] = aVector.x*aVector.x*(1-cosAngle)+cosAngle;
	result.myMatrix[1] = aVector.x*aVector.y*(1-cosAngle)+aVector.z*sinAngle;
	result.myMatrix[2] = aVector.x*aVector.z*(1-cosAngle)-aVector.y*sinAngle;

	result.myMatrix[3] = aVector.x*aVector.y*(1-cosAngle)-aVector.z*sinAngle;
	result.myMatrix[4] = aVector.y*aVector.y*(1-cosAngle)+cosAngle;
	result.myMatrix[5] = aVector.y*aVector.z*(1-cosAngle)+aVector.x*sinAngle;

	result.myMatrix[6] = aVector.x*aVector.z*(1-cosAngle)+aVector.y*sinAngle;
	result.myMatrix[7] = aVector.y*aVector.z*(1-cosAngle)-aVector.x*sinAngle;
	result.myMatrix[8] = aVector.z*aVector.z*(1-cosAngle)+cosAngle;

	return result;
}

template <typename TYPE>
Matrix33<TYPE> Matrix33<TYPE>::Identity()
{
	return Matrix33<TYPE>();
}

#endif
